#pragma once

#include "rclcpp/rclcpp.hpp"
#include <agv_actions/action/peripheral.h>
#include <agv_msgs/msg/action.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/agv_info.hpp>
#include <agv_msgs/msg/agv_event.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/load.hpp>
#include <vector>
#include <thread>
#include <mutex>
#include <agv_peripheral_controller/ioLift/io_lifter_controller.h>
#include <agv_peripheral_controller/peripheral_event.h>
// 外设事件上报的节点ID
#define PERIPHERAL_NODE_ID 2
namespace agv_peripheral
{
  // 外设逻辑层
  class CPeripheral
  {
  private:
    // 支持的操作指令
    enum OpCode
    {
      NO_OP,
      UP,
      DOWN
      // CLOCKWISE_ROTATION,
    };

    // 车体的手自动等操作模式
    // enum AgvOpMode
    // {
    //   MODE_AUTO_OFFLINE = 0,
    //   MODE_MANUL = 1,
    //   MODE_SEMIAUTO = 2,
    //   MODE_AUTO_ONLINE = 3
    // };

    // 自动模式下操作结果返回值
    enum OpResult
    {
      SUCCESS,
      FAILED,
      CANCELED,
      OTHER
    };

    // 自动运行时阶段
    enum AutoRunStage
    {
      STAGE_IDLE,
      STAGE_RUN,
      STAGE_END,
      STAGE_FAILED
    };

    struct periph_input_status
    {
      bool upControl;
      bool downControl;
    }input_status;

    // ros下的动作服务端类型定义
    typedef actionlib::SimpleActionServer<agv_actions::PeripheralAction> ACTION_SERVER_TYPE;

    // ros下动作实时反馈类型
    typedef agv_actions::PeripheralFeedback ACTION_FEED_BACK_TYPE;

    // ros下动作结果类型
    typedef agv_actions::PeripheralResult ACTION_RESULT_TYPE;

    // 处理急停事件
    // bool emgButtonState;
  public:
    // 默认构造函数
    CPeripheral(const std::string &name);

    // 析构函数
    ~CPeripheral();

    // 初始化函数
    bool init();

    // 运行函数
    void running();

  private:
    // 私有节点句柄
    rclcpp::Node m_priNh;

    // 服务端对象
    ACTION_SERVER_TYPE m_server;

    // 动作反馈对象
    ACTION_FEED_BACK_TYPE m_actionFeedBack;

    // 动作结果对象
    ACTION_RESULT_TYPE m_actionResult;

    // IO举升设备控制对象
    CIoLifterController m_lift;

    // 当前的Action 动作
    agv_msgs::msg::V_action m_curAction;

    // 手柄指令订阅
    ros::Subscriber m_periTelopSub;

    // io举升信号读取
    ros::Subscriber m_ioInputNewSub;

    // 手自动模式订阅
    ros::Subscriber m_agvInfoSub;

    // 事件订阅:获取急停信号事件
    ros::Subscriber m_eventSub;

    // 外设状态发布
    ros::Publisher m_periStatusPub;

    // 当前操作模式
    uint8_t m_curMode;

    // 控制模式锁
    std::mutex m_modeMtx;

    // 手动操作码
    CDataTimeStamp<OpCode> m_manulOpCode;

    // 自动操作码
    CDataTimeStamp<OpCode> m_autoOpCode;

    // 自动运行阶段
    AutoRunStage m_curStage;

    // 事件发布
    PeripheralEvent m_event;

    // 动作超时时间,单位毫秒ms
    int actionTimeout_;

  private:
    // 复位相关变量和状态
    void reset();

    // 加载参数
    void loadParam();

    // 动作服务端接收动作指令回调函数
    void goalCB();

    // 抢占式回调函数：这个回调可能在动作服务器需要取消或中止正在进行的操作时被调用
    void preemptCB();

    // 外设手柄指令回调函数
    void periphTeleopCtlCB(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg);

    void readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg);

    // 操作模式回调函数
    void agvInfoCB(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg);

    // 设置实时反馈数据
    void setFeedBack(OpCode code, float percent);

    // 设置反馈结果
    void setResult(OpResult result, std::string msg);

    // 设置取消任务反馈
    void setPreempt(OpResult result, std::string msg);

    // 发布者是否有效
    bool publiserValid(const ros::Publisher &pub);

    // 外设状态发布
    void periphStatusPub();

    // 外设事件发布
    void eventPub(bool occur, const std::string &msg);

    // 执行操作
    bool execOp(int code);

    // 自动运行循环
    void autoRun();

    // 手动运行循环
    void manulRun();

    // 其他操作模式运行
    void otherRun();
  };
} // agv_peripheral